# -- coding:utf-8 --
from MiniBotControlLib import ControlLib

from lib.Helper import Helper
import time


######################################################################
######################################################################
####################### 机器人基础模型 ###########################
######################################################################
######################################################################
class RobotBase:
    #声明动态库辅助对象
    mclib = ControlLib("udp_mode")

    #当前连接的机器人IP与端口Port
    robotIp = ""
    robotPort = 0
    moduleIds = []

    print("Welcome to Robot Sample....")
    # print("Enter any key to scan robots ....")
    # input("")

    ######################################################################
    ####################### 连接目标机器人设置 ###########################
    ######################################################################
    def ConnectRobot(self,ip,port):
        #连接机器人设备
        res = mclib.ConnectWithTimeOut(ip,port,200)
        if(res > 0):
            print("Connected-----OK")
            self.robotIp = ip
            self.robotPort = port
            return 1
        else:
            print("Connect Fail")
            return 0
        pass



    ######################################################################
    ####################### 机器人连接及模块扫描 ###########################
    ######################################################################
    def ScanRobotsToConnect(self):
        #扫描在线机器人Start
        robotsIps = []
        robotsCount = 0
        while(1):
         cmd = 0
         if(robotsCount > 0):
             print("Enter the robot index (ex: 1)...[Input 0 to scan again]：")
             cmd = Helper.inputNum()
             
         if(cmd == 0):
              print("Scaning the robots......")
        
              #扫描在线机器人
              #robotsIps = self.mclib.ScanRobots()
              robotsIps = self.mclib.ScanRobotsWithPort(48899)
              robotsCount = len(robotsIps)
              print("Found robots：" + str(robotsCount))
              i = 0
              while i < robotsCount:
                print(str(i + 1) + ":" + str(robotsIps[i]))
                i = i + 1
        
         elif(cmd > 0 and cmd <= robotsCount):
             robotAddr = robotsIps[cmd - 1]
             ipPort = robotAddr[:robotAddr.index(",")]     #IP与端口
             ip = ipPort
             port = 8899
             if(ipPort.find(":") > 0):
                ip = ipPort[:ipPort.index(":")]
                port = int(ipPort[ipPort.index(":") + 1:])
        
             #连接机器人设备
             rs = self.ConnectRobot(ip,port)
             print("rs：" + str(rs))
             if(rs==1):                 
                 break
        print("pass")
        return
        

    ######################################################################
    ################# 机器人模块ID及位置扫描 #############################
    ######################################################################
    def ScanIdsWithPostions(self):
        print("Begin to Scan Ids.....")
        intIds = []
        intPos = []
        while(1):
           ids = self.mclib.ScanId(20,50)
           idsCount = len(ids[0])
           intIds = [0] * idsCount
           intPos = [0] * idsCount
           print("Modules Total Count： " + str(idsCount))
           print("Followed：")
           
           c = 0
           while c < idsCount:
                intIds[c] = int(str(ids[0][c]))
                intPos[c] = int(str(ids[1][c]))
                print("Id： " + str(intIds[c]) + "  Position:" + str(intPos[c]))
                c = c + 1
           
           #默认提示扫描机器人模块ID列表
           cmd = 0
           #print("Any Key to continue....... (Input 0 to scan again)")
           #cmd = Helper.inputNum()
           #if(cmd != 0):
           break
        return intIds,intPos


    ######################################################################
    ################# 机器人模块ID扫描 ##################################
    ######################################################################
    def ScanIds(self):
       idps = self.ScanIdsWithPostions()
       return idps[0]
    
    
    ######################################################################
    ################# 机器人模块ID配置 ##################################
    ######################################################################
    def ConfigIds(self,idsortTips=[]):
        #输入操作臂ID次序
        print("Config The ids sequence:")
        self.moduleIds = []
        idsCount = 0
        
        tipslen = len(idsortTips)
        while(1):
          idsCount = idsCount + 1
          idt = str(idsCount)
          if(idsCount - 1 < tipslen):
              idt = idt + " " + idsortTips[idsCount - 1]
          print("Please enter the " + idt + " module's Id（Zero[0] Or ENTER input to complete）")
          cmd = Helper.inputNum()
          if(cmd <= 0):
            break
          self.moduleIds.append(cmd)
          
        print("Ids  completed")
        idsLen = len(self.moduleIds)
        return idsLen


    ######################################################################
    ################# 根据位置检索ID列表 #################################
    ######################################################################
    def QueryIds(self,pos,ids,postions,default=[0]):
        targetIds = []
        size = len(postions)
        for i in range(size):
            if(postions[i] == pos):
               targetIds.append(ids[i])
            i = i + 1
        if(len(targetIds) == 0):
            targetIds = default
        return targetIds

    
